/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include "IMUFileReader.hpp"


CIMUFileReader::CIMUFileReader()
 :iFileReaded(false),
  iData(),
  iTimes(),
  iBegin(),
  iEnd(),
  iMaCIGroup(""),
  iInstanceName("")
{

}
//*****************************************************************************

CIMUFileReader::~CIMUFileReader()
{
  for(EACH_IN_i(iData)){

    delete(i->second);
   
  }
   iData.clear();
   iTimes.clear();
}
//*****************************************************************************

bool CIMUFileReader::ReadFile(std::string aFileName)
{
  if(iFileReaded){
    for(EACH_IN_i(iData)){
      delete(i->second);
    }
    iData.clear();
    iTimes.clear();
  }
  
  dPrint(3,"Opening file '%s'",aFileName.c_str());
  FILE *file= fopen(aFileName.c_str(),"rt");
  char c[200];
  char maciGroup[256];
  char interfaceName[64];
  //Read the fourteen first lines..and get the interfacename...
  fgets (c, 200,file );

  fscanf(file,"%s %s %s %s",c,maciGroup,c,interfaceName);
  for(int i = 0; i< 12; i++){
    fgets (c, 200,file );
  }
  iMaCIGroup = std::string(maciGroup);
  iInstanceName = std::string(interfaceName);

  
 
  bool result = ReadData(file);
  iFileReaded = result;
  fclose(file);
  return result;

}
//*****************************************************************************
  
bool CIMUFileReader::GetServiceInformation(std::string &aMaCIGroup,
                                           std::string &aInstanceName)
{
  if(iFileReaded){
    aMaCIGroup = iMaCIGroup;
    aInstanceName = iInstanceName;
  }else{
    dPrint(1,"file not readed, could not give service information");
  }

  return iFileReaded;

}

//*****************************************************************************

bool CIMUFileReader::GetTimes(std::list<gim::time> &aTimes)
{
  if(iFileReaded){
    aTimes.assign(iTimes.begin(),iTimes.end());
    return true;
  }else{
    dPrint(1,"File not read, could not give times");
    return false;
  }
}
//*****************************************************************************

bool CIMUFileReader::GetTimeLimits(gim::time &aBegin,gim::time &aEnd)
{
  if(iFileReaded){
    aBegin = iBegin;
    aEnd = iEnd;
    return true;
  }else{
     dPrint(1,"File not read, could not give time limits");
    return false;
  }
}
//*****************************************************************************

bool CIMUFileReader::GetAllData(TIMUDataMap &aData)
{
  if(iFileReaded){
    aData.insert(iData.begin(),iData.end());
    return true;
  }else{
    dPrint(1,"File not read, could not give data");
    return false;
  }
}
//*****************************************************************************
  

bool CIMUFileReader::GetIMUData(MaCI::IMU::CIMUData &aData,
                                gim::time aTime)
{
  if(iFileReaded){
    std::map<gim::time,MaCI::IMU::CIMUData *>::iterator iter
     =iData.find(aTime);
   
   if(iter != iData.end()){
     aData = (*(iter->second));
     return true;
   }else{
     dPrint(1,"No data at wanted time  %u %u...",aTime.seconds,aTime.useconds);
     return false;
   }
  }else{
   dPrint(1,"File not read, could not give data");
    return false;
  }
}
//*****************************************************************************

bool CIMUFileReader::ReadData(FILE *iFile)
{
  using namespace MaCI::IMU;
  MaCI::Common::TTimestamp timestamp;
  MaCI::Common::TTimestamp lastTimestamp;
  int number = 1;
  unsigned int command;
  bool first = true;
  MaCI::IMU::CIMUData *imudata = NULL;
  while(number != EOF){
    lastTimestamp = timestamp;
    number = fscanf(iFile,"%u %u %u",
                    &command,
                    &(timestamp.timestamp_s),
                    &(timestamp.timestamp_us));
   
    if(first && number == 3){
      
      iBegin = timestamp.GetGimTime();
      first = false;
    }


    if(lastTimestamp.timestamp_s != timestamp.timestamp_s ||
       lastTimestamp.timestamp_us != timestamp.timestamp_us){
      if(imudata != NULL){
        iTimes.push_back(lastTimestamp.GetGimTime());
        iData[lastTimestamp.GetGimTime()] = imudata;
      }

      imudata = new CIMUData();
      imudata->CreateInternalBinBag();
      imudata->SetCommand(TCommand(KCommandEvent));
      imudata->SetTimestamp(timestamp);    
    }
    //    dPrint(1,"timestamp %s, number %d",timestamp.GetGimTime().toString().c_str(),number);




    if(number >0){

      switch(command){
      case 0:{
        float accX,accY,accZ;
        number = fscanf(iFile,"%f %f %f",
                        &accX,
                        &accY,
                        &accZ);
        imudata->SetAcceleration3d(TAcceleration3d(accX,
                                                   accY,
                                                   accZ));
        break;
      }
      case 1:{
        float rotX,rotY,rotZ;
        number = fscanf(iFile,"%f %f %f",
                        &rotX,
                        &rotY,
                        &rotZ);
        imudata->SetAngularVelocity3d(TAngularVelocity3d(rotX,
                                                         rotY,
                                                         rotZ));
        
        break;
      }
      case 2:{
        float roll,pitch,yaw;
        number = fscanf(iFile,"%f %f %f",
                        &roll,
                        &pitch,
                        &yaw);
        imudata->SetAngle3d(TAngle3d(roll,pitch,yaw));
        break;
      }
      case 3:{
        float x,y,z,scalar;
        number = fscanf(iFile,"%f %f %f %f",
                        &scalar,
                        &x,
                        &y,
                        &z);

        imudata->SetQuaternion(TQuaternion(scalar,
                                           x,
                                           y,
                                           z));
        break;
      }
      case 4:{
        float e11,e12,e13,e21,e22,e23,e31,e32,e33;
        number = fscanf(iFile,"%f %f %f %f %f %f %f %f %f",
                        &e11,&e12,&e13,
                        &e21,&e22,&e23,
                        &e31,&e32,&e33);
        imudata->SetOrientationMatrix(TOrientationMatrix(e11,e12,e13,
                                                         e21,e22,e23,
                                                         e31,e32,e33));
        break;
      }
        
        
      }

     
     
    
    }
    

  }
  if(imudata != NULL){
    iTimes.push_back(timestamp.GetGimTime());
    iData[lastTimestamp.GetGimTime()] = imudata;
  }
  if(!first){
    iEnd = timestamp.GetGimTime();
  }

  return true;

}
//*****************************************************************************

bool CIMUFileReader::GetAcceleration3d( MaCI::IMU::TAcceleration3d &aAcceleration,
                                       gim::time aTime)
{
  if(iFileReaded){
    std::map<gim::time,MaCI::IMU::CIMUData *>::iterator iter
      =iData.find(aTime);
    
    if(iter != iData.end()){
      const MaCI::IMU::TAcceleration3d *acc =iter->second->GetAcceleration3d();

      if(acc){

        aAcceleration = *acc;
        return true;
      }else{
        dPrint(1,"No acceleration element at wanted time (%u %u)",aTime.seconds,aTime.useconds);
        return false;
      }
    }else{
      dPrint(1,"No data at wanted time  %u %u...",aTime.seconds,aTime.useconds);
      return false;
    }
  }else{
    dPrint(1,"File not read, could not give data");
    return false;
  }
}
//*****************************************************************************

bool CIMUFileReader::GetAngularVelocity3d( MaCI::IMU::TAngularVelocity3d &aAngularVelocity,
                                          gim::time aTime)
{
 if(iFileReaded){
    std::map<gim::time,MaCI::IMU::CIMUData *>::iterator iter
      =iData.find(aTime);
    
    if(iter != iData.end()){
      const MaCI::IMU::TAngularVelocity3d *ang  = (iter->second->GetAngularVelocity3d());
      if(ang){
        aAngularVelocity = *ang;
        return true;
      }else{
        dPrint(1,"No angular velocity element at wanted time (%u %u)",aTime.seconds,aTime.useconds);
             
        return false;
      }
    }else{
      dPrint(1,"No data at wanted time  %u %u...",aTime.seconds,aTime.useconds);
      return false;
    }
  }else{
    dPrint(1,"File not read, could not give data");
    return false;
  }
}
//*****************************************************************************

bool CIMUFileReader::GetAngle3d(MaCI::IMU::TAngle3d &aAngle,
                                gim::time aTime)
{
 if(iFileReaded){
    std::map<gim::time,MaCI::IMU::CIMUData *>::iterator iter
      =iData.find(aTime);
    
    if(iter != iData.end()){
      const MaCI::IMU::TAngle3d *ang = (iter->second->GetAngle3d());
      if(ang){
        aAngle = *ang;
        return true;
      }else{
        dPrint(1,"No acceleration element at wanted time (%u %u)",aTime.seconds,aTime.useconds);
        return false;
      }
    }else{
      dPrint(1,"No data at wanted time  %u %u...",aTime.seconds,aTime.useconds);
      return false;
    }
  }else{
    dPrint(1,"File not read, could not give data");
    return false;
  }
}
//*****************************************************************************

bool CIMUFileReader::GetQuaternion(MaCI::IMU::TQuaternion &aQuaternion,
                                   gim::time aTime)
{
 if(iFileReaded){
    std::map<gim::time,MaCI::IMU::CIMUData *>::iterator iter
      =iData.find(aTime);
    
    if(iter != iData.end()){
      const MaCI::IMU::TQuaternion *quat = (iter->second->GetQuaternion());

      if(quat){
        aQuaternion = *quat;
        return true;
      }else{
        dPrint(1,"No Quaternion element at wanted time (%u %u)",aTime.seconds,aTime.useconds);

        return false;
      }
    }else{
      dPrint(1,"No data at wanted time  %u %u...",aTime.seconds,aTime.useconds);
      return false;
    }
  }else{
    dPrint(1,"File not read, could not give data");
    return false;
  }
}
//*****************************************************************************

bool CIMUFileReader::GetOrientationMatrix(MaCI::IMU::TOrientationMatrix &aOrientationMatrix,
                                          gim::time aTime)
{
 if(iFileReaded){
    std::map<gim::time,MaCI::IMU::CIMUData *>::iterator iter
      =iData.find(aTime);
    
    if(iter != iData.end()){
      const MaCI::IMU::TOrientationMatrix *ori =  (iter->second->GetOrientationMatrix());
      if(ori){
        aOrientationMatrix = *ori;
        return true;
      }else{
        dPrint(1,"No Orientation matrix element at wanted time (%u %u)",aTime.seconds,aTime.useconds);
        return false;
      }
    }else{
      dPrint(1,"No data at wanted time  %u %u...",aTime.seconds,aTime.useconds);
      return false;
    }
  }else{
    dPrint(1,"File not read, could not give data");
    return false;
  }
}
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
